#include "ManejadorVRPN.h"


ManejadorVRPN::ManejadorVRPN() 
{
	simpleApp = SimpleApp::GetInstance();
	offsetX = 0;
	offsetY = 0;
	lastReadingX = 525;
	lastReadingY = 425;
}

void ManejadorVRPN::analogEvent(const vrpn_ANALOGCB a){
	
	float vR=0, vT=0;
	float ang = a.channel[NUNCHUK_JOYSTICK_ANGLE];
	float mag = a.channel[NUNCHUK_JOYSTICK_MAG];

	// Reviso que los valores obtenidos del nunchuk sean validos antes de usarlos
	if(isNumber(ang) && isNumber(mag)){
		if (mag < 0.3)
			mag = 0;
		vT = mag*cos(ang*PI/180.0f);
		vR = -mag*sin(ang*PI/180.0f)*2;
		Fantasma* fantasma = simpleApp->getFantasmaPrincipal();
		fantasma->cambiarVelocidad(vR,vT);
	}
	
	float sensorReadings[4][2];
	sensorReadings[0][0] = a.channel[FIRST_SENSOR_X];
	sensorReadings[1][0] = a.channel[SECOND_SENSOR_X];
	sensorReadings[2][0] = a.channel[THIRD_SENSOR_X];
	sensorReadings[3][0] = a.channel[FOURTH_SENSOR_X];
	sensorReadings[0][1] = a.channel[FIRST_SENSOR_Y];
	sensorReadings[1][1] = a.channel[SECOND_SENSOR_Y];
	sensorReadings[2][1] = a.channel[THIRD_SENSOR_Y];
	sensorReadings[3][1] = a.channel[FOURTH_SENSOR_Y];
	float avgX = 0, avgY = 0;
	float validReadings = 0;
	//printf("Sensor Readings:%f\n",a.channel[0]);
	for(int i = 0; i < 4; i++){
		//printf("%f , %f\n",sensorReadings[i][0],sensorReadings[i][1]);
		if(sensorReadings[i][0]>=0 && sensorReadings[i][1]>=0){
			avgX += sensorReadings[i][0];
			avgY += sensorReadings[i][1];
			validReadings++;
		}
	}
	if(validReadings == 0){
		//No pude detectar la posicion del control
		//printf("No te encuentro!");
		simpleApp->placeCrosshair(0.0f,0.0f);
	}else{
		// Calculo el promedio
		avgX = avgX/validReadings;
		avgY = avgY/validReadings;
		
		lastReadingX = avgX;
		lastReadingY = avgY;

		// Centro
		//avgX-=250;
		//avgY-=500;
		avgX-=offsetX;
		avgY-=offsetY;

		// Invierto el Y
		avgY*=-1;

		// Normalizo
		avgX/=60;
		avgY/=60;
		simpleApp->setCrosshair(avgX,avgY);

	}
	
}

void ManejadorVRPN::buttonEvent(const vrpn_BUTTONCB b){
	Fantasma* fantasma = simpleApp->getFantasmaPrincipal();
	float v = fantasma->getVelVertical();
	float vel = 0;
	switch(b.button){
	case BOTON_1:
		simpleApp->cambiarCamaraSeleccionada(0);
		break;
	case BOTON_2:
		simpleApp->cambiarCamaraSeleccionada(1);
		break;
	case BOTON_C:
		if(b.state==1)
			vel++;
		else
			vel--;				
		v+=vel;
		fantasma->cambiarVelocidadVertical(v);
		break;
	case BOTON_Z:		
		if(b.state==0)
			vel++;
		else
			vel--;
		v+=vel;
		fantasma->cambiarVelocidadVertical(v);
		break;
	case BOTON_A:
		if(b.state==1)
			simpleApp->capturarMoneda();
		else
			simpleApp->soltarMoneda();
		break;
	case BOTON_B:		
		offsetX = lastReadingX;
		offsetY = lastReadingY;
		break;
	case BOTON_HOME:
		simpleApp->iniciar();
		break;
	}


}
